﻿#pragma once
#include <pcl/point_cloud.h>
#include <pcl/pcl_base.h>
#include <vector>
#include <Eigen/Core>

using namespace std;

namespace oil
{
template <typename PointT>
class PyramidExclusive : public pcl::PCLBase<PointT>
{
public:
	typedef pcl::PointCloud<PointT> PointCloud;
	typedef typename pcl::PointCloud<PointT>::Ptr PointCloudPtr;
	typedef typename pcl::PointCloud<PointT>::ConstPtr PointCloudConstPtr;
	typedef boost::shared_ptr< PyramidExclusive<PointT> > Ptr;
	typedef boost::shared_ptr< const PyramidExclusive<PointT> > ConstPtr;

public:
	/*************************************************************************/
	/**
	* \brief 构造函数
	* \Param[in]		float min_window 最小窗口，一般由点云密度决定
	* \Param[in]		float max_window 最大窗口，一般由场景中最大物体决定
	* \Param[im]		float step		 窗口增长比例
	*/
	/*************************************************************************/
	PyramidExclusive(float min_window, float max_window, float step = 1.41);
	~PyramidExclusive();

	/*************************************************************************/
	/**
	* @Synopsis		getInputCloud				获取输入点云
	* @Returns		PointCloudConstPtr const	返回点云指针
	* @remark		获取点云
	*/
	/*************************************************************************/
	inline PointCloudConstPtr const	getInputCloud() { return (input_); }

	/*************************************************************************/
	/**
	* @Synopsis		setMinWindowSize			设置最小窗口大小
	* @Param		float min_window
	* @Returns		void
	* @remark		最小窗口，一般由点云密度决定
	*/
	/*************************************************************************/
	inline void	setMinWindowSize(float min_window) { min_window_ = min_window; }

	/*************************************************************************/
	/**
	* @Synopsis		getMinWindowSize			获取最小窗口大小
	* @Returns		float
	* @remark
	*/
	/*************************************************************************/
	inline float getMinWindowSize() const { return (min_window_); }

	/*************************************************************************/
	/**
	* @Synopsis		setMaxWindowSize			设置最大窗口大小
	* @Param		float max_window
	* @Returns		void
	* @remark		最大窗口一般由场景中最大物体决定
	*/
	/*************************************************************************/
	inline void	setMaxWindowSize(float max_window) { max_window_ = max_window; }

	/*************************************************************************/
	/**
	* @Synopsis		getMaxWindowSize			获取最大窗口大小
	* @Returns		float
	* @remark
	*/
	/*************************************************************************/
	inline float getMaxWindowSize() const { return (max_window_); }

	/*************************************************************************/
	/**
	* @Synopsis		setDistanceThreshold		设置判断点相等的阈值
	* @Param		float threshold				阈值大小
	* @Returns		void
	* @remark		单位为米，如0.01m
	*/
	/*************************************************************************/
	inline void	setDistanceThreshold(float threshold) { threshold_ = threshold; }

	/*************************************************************************/
	/**
	* @Synopsis		getMaxWindowSize			获取阈值大小
	* @Returns		void
	*/
	/*************************************************************************/
	inline float getDistanceThreshold() const { return (threshold_); }

	/*************************************************************************/
	/**
	* @Synopsis		compute						计算规则格网点云金字塔
	* @Param		std::vector<PointCloudPtr> & output 一组格网化的点云
	*/
	/*************************************************************************/
	void compute(std::vector<PointCloudPtr>& output);

	/*************************************************************************/
	/**
	* @Synopsis		compute						计算规则格网点云金字塔
	* @Param		std::vector<int> & indices	一组格网化点云的input点云索引
	* @Returns		void
	* @remark
	*/
	/*************************************************************************/
	void compute(vector<vector<int>>& indices);
	/*************************************************************************/
	/**
	* @Synopsis		getClassName				获取类名
	* @Returns		const std::string&
	*/
	/*************************************************************************/
	inline const std::string& getClassName() const { return (name_); }

	/*************************************************************************/
	/**
	* @Synopsis		getStepLevels				获取金字塔窗口大小
	* @Returns		const std::vector<float>&
	* @remark		窗口大小是一个数组，和金字塔层数对应
	*/
	/*************************************************************************/
	inline const std::vector<float>& getStepLevels() const { return levels_; }

	/*************************************************************************/
	/**
	* @Synopsis		getMin3D					返回点云的最小点
	* @Returns		Eigen::Vector4f
	*/
	/*************************************************************************/
	Eigen::Vector4f getMin3D() const { return min_p_; }

	/*************************************************************************/
	/**
	* @Synopsis		getMax3D					返回点云的最大点
	* @Returns		Eigen::Vector4f
	*/
	/*************************************************************************/
	Eigen::Vector4f getMax3D() const { return max_p_; }
private:
	/*************************************************************************/
	/**
	* @Synopsis		initCompute					初始化操作
	* @Returns		bool
	* @remark		计算窗口值，点云包围盒
	*/
	/*************************************************************************/
	bool initCompute();

	/*************************************************************************/
	/**
	* @Synopsis		organizeInput				格网化某一级点云
	* @Param		const PointCloudPtr & input 非规则格网点云
	* @Param		float window_size			格网窗口大小
	* @Param		PointCloudPtr & output		规则格网
	* @Returns		void
	* @remark
	*/
	/*************************************************************************/
	void organizeInput(const PointCloudPtr& input, float window_size, PointCloudPtr& output);

	/*************************************************************************/
	/**
	* @Synopsis		organizeInput				格网化某一级点云
	* @Param		const std::vector<int> & input 未格网化点的索引
	* @Param		float window_size			格网窗口大小
	* @Param		std::vector<int> & output	格网化的点的索引
	* @Returns		void
	* @remark
	*/
	/*************************************************************************/
	void organizeInput(const std::vector<int>& input, float window_size, std::vector<int>& output);

	/*************************************************************************/
	/**
	* @Synopsis		removeNaN					移除NaN值
	* @Param		PointCloudPtr & output
	* @Param		float window_size
	* @Returns		void
	* @remark		在计算格网点云时，会先对每个点云复制为NaN
	*/
	/*************************************************************************/
	void removeNaN(PointCloudPtr& output, float window_size);

	void checkNaN(const PointCloudPtr& cloud);

	/*************************************************************************/
	/**
	* @Synopsis		getVonNeumannNeighbour		获取四邻域内非NaN点
	* @Param	const PointCloudPtr & input
	* @Param		int row						点行号
	* @Param		int col						列号
	* @Param		PointT & out_pt				输出的非nan点
	* @Returns		void
	* @remark		递归查找四邻域中的非NaN
	*/
	/*************************************************************************/
	void getVonNeumannNeighbour(const PointCloudPtr& input, int row, int col, PointT& out_pt);

	/*************************************************************************/
	/**
	* @Synopsis		nullify						将一个点设置成nan
	* @Param		PointT & p
	* @Returns		void
	* @remark
	*/
	/*************************************************************************/
	inline void	nullify(PointT& p) const
	{
		p.x = p.y = p.z = std::numeric_limits<float>::quiet_NaN();
	}

private:

	// 窗口大小
	std::vector<float> levels_;

	// 窗口增长系数
	float step_;

	// 最小窗口
	float min_window_;

	// 最大窗口
	float max_window_;

	/// 类名
	std::string name_;

	/// 判断点相等的阈值
	float threshold_;

	//包围盒
	Eigen::Vector4f min_p_;
	Eigen::Vector4f max_p_;

public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

}//end oil

#include "pyramid_exclusive.hpp"

